import numpy as np
import scipy
from exponential_matrix_helperFromConsim import ExponentialMatrixHelper


emh = ExponentialMatrixHelper()
emh.use_new_expm_alg = False


A = np.array([
    [20.4113,  -1.35618,   -29.022,  0.775944,    1.6047,   1.55972,  -15.7032,   1.89639,   11.2717,   27.3881],
    [-6.33702,   7.73226,  -15.4268,   20.3467,   16.1948,  -24.8366,    28.238,  -27.6432,  -20.0416,   5.31841],
    [16.986,  -8.11293,  -21.7661,   6.75839,  -5.98628,  -18.4672,   24.1325,  -3.74174,  -3.59373,   9.43824],
    [17.9064,  0.804055,   18.2506,  -12.2381,   23.4918,   9.79362,   21.0552,   25.9101,   22.8045,   21.5206],
    [24.6988,   27.1338,  -20.5993,   8.25314,  -13.0011,    23.414,  -14.0001,   25.8486,   19.7521,   -3.6264],
    [-18.1469,   24.9717,  -5.94334,   1.45723,   -8.8525,  -9.06642,   2.38562,   13.2571,  -10.1798,   25.4382],
    [-9.88663,    8.1427,  -22.2126, -0.385021,   18.4635,  -26.1497,  -7.48758,  -12.9424,  -16.2619,   -6.0938],
    [16.0938,   13.0378,  -23.4715,   28.3665,   25.1416,  -28.7986,   15.6149,   14.3121,   23.6023,    18.886],
    [-13.3335,  -21.5038,   29.9355,   -12.449,  -25.8147,   -2.5379,  0.752122,   8.39873,  -8.97839,   11.0531],
    [3.2382,   6.41813,  -16.9046,   16.2815,   26.9596,  -26.2142,   10.0634,  -8.75708,   11.2002,   24.6583]
])

print(A)
APos = -A @ A.T
print(APos)
print(np.linalg.eig(APos)[0])

Aref = scipy.linalg.expm(APos)
print(Aref)

for i in range(20):
    res = emh.expm(APos, i, False)

    errNorm = np.linalg.norm(Aref - res)
    print("Max Multiplications: {} error: {}".format(i, errNorm))
